#include "forward_kinematics.h"
#include "motor.h" /* 若你把零点/限位放在 motor.h */
#include "mqtt_comm.h"
#include "stdio.h"
#include "term.h"
#include <math.h>
#include <string.h>

/* 结构参数（单位：m） —— 与 Matlab 使用的值一致 */
static const double L1 = 148e-3;
static const double L2 = 42e-3;
static const double L3 = 132e-3;
static const double L4 = 122e-3;
static const double L5 = 108e-3;
static const double L6 = 78e-3; /* 注意：Matlab 使用 78e-3 */

/* 转换比例（默认与 Matlab 中 Motor_position_pulse_and_DH_parameters 一致） */
static const double POSITION_PULSE_PER_M = 1.0 / 50e-9; /* 1 脉冲 = 50 nm */
static const double ROTATIONAL_PULSE_PER_DEG_DD =
    1.0 / 0.001; /* 1 脉冲 = 0.001° */

/* 注意：关于 theta6 的旋转脉冲比例，Matlab 的两个脚本中不一致：
 *  - 在逆运动学的示例中使用过 1/0.0002475921753
 *  - 在正运动学的 Motor 参数里把 theta6（光栅）用的是
 * 1/0.0000944702（注释为光栅） 这里的 forward_kinematics_from_pulses_matlab
 * 使用 "光栅" 的比例来匹配 Matlab FK 的示例（如果你的 pulse_theta6
 * 来源是电机编码器而非光栅，请改用上面的值）。
 */
static const double ROTATIONAL_PULSE_PER_DEG_ZOLIX_GRATING =
    1.0 / 0.0000944702; /* grating -> deg */

/* 各轴零点脉冲数（示例：用 Matlab FK 文件里的值，实物以测量值为准） */
extern int
    ZERO_PULSE_THETA6; /* 可能在其它模块定义并初始化（例如依据开机校准） */
static const int ZERO_PULSE_THETA5 = 71158;
static const int ZERO_PULSE_D4 = 795823;
static const int ZERO_PULSE_D3 = 703125;
static const int ZERO_PULSE_D2 = 705678;
static const int ZERO_PULSE_THETA1 = 226148;

/* 外部状态（和你原来代码一致） */
extern double actual_theta6_angle; /* 摆台实际角度（若你有这个变量） */

/* -------------------------
 * helper: clamp for asin input
 * ------------------------- */
static double clamp_double(double v, double lo, double hi) {
  if (v < lo)
    return lo;
  if (v > hi)
    return hi;
  return v;
}

/**
 * ideal_DH_FK:
 *   与 Matlab 中 ideal_DH_FK 等价的实现。
 *
 * 输入：
 *   theta1_deg, d2_m, d3_m, d4_m, theta5_deg, theta6_deg
 *   obj_pos[6] : obj_pos[0..2] in μm, obj_pos[3..5] in degrees
 *
 * 输出：
 *   out_x_um, out_y_um, out_z_um （单位 μm）
 *   out_alpha_deg, out_beta_deg, out_gamma_deg （单位 °）
 */
void ideal_DH_FK(double theta1_deg, double d2_m, double d3_m, double d4_m,
                 double theta5_deg, double theta6_deg, const double obj_pos[6],
                 double *out_x_um, double *out_y_um, double *out_z_um,
                 double *out_alpha_deg, double *out_beta_deg,
                 double *out_gamma_deg) {
  /* 转换角度到弧度 */
  double t1 = theta1_deg * (M_PI / 180.0);
  double t5 = theta5_deg * (M_PI / 180.0);
  double t6 = theta6_deg * (M_PI / 180.0);

  /* 构造 T_e0 （Matlab 中的 T_e0） */
  double T_e0[4][4];
  /* 第一行 */
  T_e0[0][0] = cos(t1) * cos(t6) + sin(t1) * sin(t5) * sin(t6);
  T_e0[0][1] = -cos(t5) * sin(t1);
  T_e0[0][2] = cos(t6) * sin(t1) * sin(t5) - cos(t1) * sin(t6);
  T_e0[0][3] = -sin(t1) * (d2_m + L6 * cos(t6) * sin(t5)) +
               cos(t1) * (d3_m + L4 + L5 + L6 * sin(t6));

  /* 第二行 */
  T_e0[1][0] = cos(t6) * sin(t1) - cos(t1) * sin(t5) * sin(t6);
  T_e0[1][1] = cos(t1) * cos(t5);
  T_e0[1][2] = -cos(t1) * cos(t6) * sin(t5) - sin(t1) * sin(t6);
  T_e0[1][3] = cos(t1) * (d2_m + L6 * cos(t6) * sin(t5)) +
               sin(t1) * (d3_m + L4 + L5 + L6 * sin(t6));

  /* 第三行 */
  T_e0[2][0] = cos(t5) * sin(t6);
  T_e0[2][1] = sin(t5);
  T_e0[2][2] = cos(t5) * cos(t6);
  T_e0[2][3] = d4_m + L1 + L2 + L3 - L6 * cos(t5) * cos(t6);

  /* 第四行 */
  T_e0[3][0] = 0.0;
  T_e0[3][1] = 0.0;
  T_e0[3][2] = 0.0;
  T_e0[3][3] = 1.0;

  /* 构造 T_Te（obj_pos -> 注意 obj_pos 的单位：前3个 μm -> m，后三个°->rad） */
  double rx = obj_pos[3] * (M_PI / 180.0);
  double ry = obj_pos[4] * (M_PI / 180.0);
  double rz = obj_pos[5] * (M_PI / 180.0);

  /* R_Te = Rx(rx) * Ry(ry) * Rz(rz) , same order as Matlab */
  double Rx[3][3] = {{1, 0, 0}, {0, cos(rx), -sin(rx)}, {0, sin(rx), cos(rx)}};
  double Ry[3][3] = {{cos(ry), 0, sin(ry)}, {0, 1, 0}, {-sin(ry), 0, cos(ry)}};
  double Rz[3][3] = {{cos(rz), -sin(rz), 0}, {sin(rz), cos(rz), 0}, {0, 0, 1}};

  double tmp[3][3] = {0}, R_Te[3][3] = {0};
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      for (int k = 0; k < 3; k++)
        tmp[i][j] += Rx[i][k] * Ry[k][j];
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      for (int k = 0; k < 3; k++)
        R_Te[i][j] += tmp[i][k] * Rz[k][j];

  /* P_Te in m (obj_pos[0..2] are μm) */
  double P_Te[3] = {obj_pos[0] * 1e-6, obj_pos[1] * 1e-6, obj_pos[2] * 1e-6};

  /* Assemble T_Te 4x4 */
  double T_Te[4][4];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++)
      T_Te[i][j] = R_Te[i][j];
    T_Te[i][3] = P_Te[i];
  }
  T_Te[3][0] = 0;
  T_Te[3][1] = 0;
  T_Te[3][2] = 0;
  T_Te[3][3] = 1;

  /* T_T0 = T_e0 * T_Te */
  double T_T0[4][4] = {0};
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      for (int k = 0; k < 4; k++)
        T_T0[i][j] += T_e0[i][k] * T_Te[k][j];

  /* 提取 R = T_T0(1:3,1:3) */
  double R[3][3];
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      R[i][j] = T_T0[i][j];

  /* Matlab 中：
     beta_T0 = asin(R(1,3));          -> R[0][2]
     gamma_T0 = atan2(-R(1,2), R(1,1)); -> atan2(-R[0][1], R[0][0])
     alpha_T0 = atan2(-R(2,3), R(3,3)); -> atan2(-R[1][2], R[2][2])
  */
  double beta = asin(clamp_double(R[0][2], -1.0, 1.0));
  double gamma = atan2(-R[0][1], R[0][0]);
  double alpha = atan2(-R[1][2], R[2][2]);

  /* 位移（m） -> 输出的单位要与 Matlab 匹配：输出 x_T0*1e6 (μm) */
  double x_T0_m = T_T0[0][3];
  double y_T0_m = T_T0[1][3];
  double z_T0_m = T_T0[2][3];

  if (out_x_um)
    *out_x_um = x_T0_m * 1e6;
  if (out_y_um)
    *out_y_um = y_T0_m * 1e6;
  if (out_z_um)
    *out_z_um = z_T0_m * 1e6;

  /* 输出角度（deg） */
  if (out_alpha_deg)
    *out_alpha_deg = alpha * (180.0 / M_PI);
  if (out_beta_deg)
    *out_beta_deg = beta * (180.0 / M_PI);

  // /* gamma 的处理：保持 Matlab 的输出 gamma_T0
  //  * 但保留你原来代码中的一个后备策略：如果光栅读取失败（任务状态 != 204），
  //  * 你原先用 actual_theta6_angle
  //  作为摆台角的“可靠读数”。这里仍然尊重该逻辑：
  //  *  - 如果 g_robotStateData.taskState == 204 : 使用 Matlab 计算的 gamma
  //  *  - 否则：用 -actual_theta6_angle （注意 actual_theta6_angle 的单位应为
  //  °）
  //  */
  if (out_gamma_deg) {
    *out_gamma_deg = gamma * (180.0 / M_PI);
    // if (g_robotStateData.taskState == 204) {
    //   *out_gamma_deg = gamma * (180.0 / M_PI);
    // } else {
    //   *out_gamma_deg = -actual_theta6_angle;
    // }
  }
}

/**
 * forward_kinematics_from_pulses:
 *  将脉冲转为角度/位移（按 Matlab 的参数），并调用 ideal_DH_FK_matlab 得到最终
 * parameter（μm/°）
 *
 *  输入：
 *    pulse_theta1,..., pulse_theta6 : 脉冲
 *    obj_pos[6] : obj_pos in μm / °
 *
 *  输出：
 *    out_x_um, out_y_um, out_z_um : μm
 *    out_alpha_deg, out_beta_deg, out_gamma_deg : °
 */
void forward_kinematics_from_pulses(int pulse_theta1, int pulse_d2,
                                    int pulse_d3, int pulse_d4,
                                    int pulse_theta5, int pulse_theta6,
                                    const double obj_pos[6], double *out_x_um,
                                    double *out_y_um, double *out_z_um,
                                    double *out_alpha_deg, double *out_beta_deg,
                                    double *out_gamma_deg) {
  /* 将电机脉冲转换为物理量（theta: °, d: m）
   * 注意：这里为 theta6 使用的是 “光栅”
   * 比例（ROTATIONAL_PULSE_PER_DEG_ZOLIX_GRATING）。 如果你的 pulse_theta6
   * 是电机编码器（非光栅），把该常数替换为正确的比率。
   */
  double theta1_deg =
      (pulse_theta1 - ZERO_PULSE_THETA1) / ROTATIONAL_PULSE_PER_DEG_DD;
  double d2_m = (pulse_d2 - ZERO_PULSE_D2) / POSITION_PULSE_PER_M;
  double d3_m = (pulse_d3 - ZERO_PULSE_D3) / POSITION_PULSE_PER_M;
  double d4_m = (pulse_d4 - ZERO_PULSE_D4) / POSITION_PULSE_PER_M;
  double theta5_deg =
      (pulse_theta5 - ZERO_PULSE_THETA5) / ROTATIONAL_PULSE_PER_DEG_DD;
  double theta6_deg = -1.0 * (pulse_theta6 - ZERO_PULSE_THETA6) /
                      ROTATIONAL_PULSE_PER_DEG_ZOLIX_GRATING;

  /* 调用 Matlab 等价的 FK */
  ideal_DH_FK(theta1_deg, d2_m, d3_m, d4_m, theta5_deg, theta6_deg, obj_pos,
              out_x_um, out_y_um, out_z_um, out_alpha_deg, out_beta_deg,
              out_gamma_deg);
}
